﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Drifting.Foundation;

namespace Drifting.MotionPlanning.Systems
{
    public class OneLink_simple: OneLink
    {
        public OneLink_simple()
            : base()
        {
            max_ctrl[0] = 1;
            min_ctrl[0] = -1;
        }


        override public void Qd(double[] q, double[] ctrl, ref double[] qd)
        {
            double torque = ctrl[0];
            qd[0] = q[1];
            qd[1] = torque * 3 / (m1 * L1 * L1) -   g * Math.Cos(q[0]) * 1.5 /  L1;
        }

        override public bool Propagate(MemoryPad pad, double[] q_start, double[] u, double duration, ref double[] q_end, ref double step_cost)
        {
            if (q_end == null)
                q_end = new double[DOF_State];

            int n_step = (int)Math.Abs(Math.Floor(duration / t_delta + 0.5));

            double t_delta_signed = t_delta;
            if(duration < 0)
                t_delta_signed = -t_delta;

            MathAssistant.vector_set(q_start, ref pad.q_tmp);
            MathAssistant.vector_set(q_start, ref q_end);
            step_cost = 0;
            for (int i = 0; i < n_step; ++i)
            {
                this.Integration(pad, pad.q_tmp, u, t_delta_signed, ref q_end);
                MathAssistant.vector_set(q_end, ref pad.q_tmp);
                step_cost += t_delta;
            }

            double angle = q_end[0];
            while (angle > Math.PI)
                angle -= MathAssistant.PI_double;
            while (angle < -Math.PI)
                angle += MathAssistant.PI_double;
            q_end[0] = angle;

            return inStateSpace(q_end);
        }
    }
}
